#include <ros/ros.h>
#include <nav_msgs/Odometry.h>

double mean_loss = 0.0;
double lamba = 0.5;
double ds_loss_max = 0.001;
double x = 0.0, y = 0.0, s = 0.0;
double vx = 0.0, vy = 0.0;
ros::Time current_time, last_time;

void disCallback(const nav_msgs::Odometry::ConstPtr &msg) {
    current_time = ros::Time::now();

    vx = msg->twist.twist.linear.x;
    vy = msg->twist.twist.linear.y;

    double dt = (current_time - last_time).toSec();
    double delta_x = vx * dt;
    double delta_y = vy * dt;
    double delta_s = sqrt(delta_x * delta_x + delta_y * delta_y);

    x += delta_x;
    y += delta_y;
    if (delta_s > 0.005) {
    s += delta_s;
}


    if (delta_s < ds_loss_max) {
        mean_loss = delta_s * lamba + mean_loss * (1 - lamba);
    }

    delta_s -= mean_loss;  // 确保 delta_s 不会变为负数
    last_time = current_time;

    ROS_INFO("v = %0.6f", vx);

    ROS_INFO("s = %0.6f", s);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "speed_subscriber");
    ros::NodeHandle n;
    current_time = ros::Time::now();
    last_time = current_time;
    
    ros::Subscriber dis_sub = n.subscribe("/odom", 100, disCallback);
    ros::spin();
    return 0;
}

